package endcourseproject;

import java.util.Random;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;


public class ProximityTest {
	
	// Limits (should not be constant)
	private static final int BACKWARD_LIMIT = 0xa0;
	private static final int FORWARD_LIMIT = 0x70;
	
	// Variables for averaging
	private static final int ARRAY_SIZE = 5;
	private static byte[] array = new byte[ARRAY_SIZE];
	private static byte index = 0;
	
	// Variables for data generation
	final static int DATA_SIZE = 4;
	private static Random random = new Random();
	/**
	 * @param args
	 */
	public static void main(String[] args) {

		// Create listener to handle incoming messages
		ZigBeeListener listener = new ZigBeeListener() {
			
			@Override
			public void message(MCPS_dataIndication message) {
				
				
				  // Get message signal
				 
				int signal = (int)(0xFF & message.getMpduLinkQuality());
				
				// Add to array
				
				/*
				array[index++] = signal;
				if(index >= ARRAY_SIZE)
				{
					index -= ARRAY_SIZE;
				}
				
				// Calculate sum
				int sum = 0;
				for(int i = 0 ; i < ARRAY_SIZE;i++)
				{
					sum += array[i];
				}
				*/

				if( signal < FORWARD_LIMIT )
				{
					// Drive forward if average is below limit
					forward();
				}
				else if (signal > BACKWARD_LIMIT)
				{
					// Drive backward if average is above limit 
					backward();
				}
			}
		};
		
		// Create device and attach listener
		ZigBeeDevice device = new ZigBeeDevice(SensorPort.S1);
		device.setListener(listener);
		
		// Create data array
		//byte[] data = new byte[DATA_SIZE];
		byte[] data = new byte[] {0x01,0x02,0x03,0x04,0x05};
		
		
		while(!Button.ESCAPE.isPressed())
		{
			// Sleep
			Common.sleep(500);
			/*
			// Make some random data
			for( int i = 0 ; i < DATA_SIZE; i++)
			{
				data [i] =(byte)random.nextInt();
			}
			*/
			// Send data via zigbee
			LCD.drawString("before ", 0, 2);

			device.BroadcastDataRequest(data);	
			//device.BroadcastDataRequest(new byte[] {  0x07, 0x08, 0x09, 0x0a, 0x0b });

			LCD.drawString("after  ", 0, 2);
		}
		
		stop();
	}

	
	private static void backward()
	{
		LCD.drawString("backward", 0, 3);
		Motor.A.backward();
		Motor.B.backward();
	}
	private static void forward()
	{
		LCD.drawString("forward ", 0, 3);
		Motor.A.forward();
		Motor.B.forward();
	}
	private static void stop()
	{
		LCD.drawString("stop    ", 0, 3);
		Motor.A.stop();
		Motor.B.stop();	
	}
}
